#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/io/pcd_io.h>
#include <flann/flann.h>
#include <flann/io/hdf5.h>
#include <boost/filesystem.hpp>
#include <iostream>
#include <string>
#include <fstream>
#include <sstream>

#include <CollisionWorldViewer\ObjectStruct.h>

template <class T>
bool from_string(T& t, 
                 const std::string& s, 
                 std::ios_base& (*f)(std::ios_base&))
{
  std::istringstream iss(s);
  return !(iss >> f >> t).fail();
}

typedef std::pair<std::string, std::vector<float> > vfh_model;
int convertVFH (int x, float * ret);
std::vector<float> displayGrasp (float * point, int number);

					
		

/** \brief Loads an n-D histogram file as a VFH signature
  * \param path the input file name
  * \param vfh the resultant VFH model
  */
bool
loadHist (const boost::filesystem::path &path, vfh_model &vfh)
{
  int vfh_idx;
  // Load the file as a PCD
  try
  {
    sensor_msgs::PointCloud2 cloud;
    int version;
    Eigen::Vector4f origin;
    Eigen::Quaternionf orientation;
    pcl::PCDReader r;
    int type; unsigned int idx;
    r.readHeader (path.string (), cloud, origin, orientation, version, type, idx);

    vfh_idx = pcl::getFieldIndex (cloud, "vfh");
    if (vfh_idx == -1)
      return (false);
    if ((int)cloud.width * cloud.height != 1)
      return (false);
  }
  catch (pcl::InvalidConversionException e)
  {
    return (false);
  }

  // Treat the VFH signature as a single Point Cloud
  pcl::PointCloud <pcl::VFHSignature308> point;
  pcl::io::loadPCDFile (path.string (), point);
  vfh.second.resize (308);

  std::vector <sensor_msgs::PointField> fields;
  getFieldIndex (point, "vfh", fields);

  for (size_t i = 0; i < fields[vfh_idx].count; ++i)
  {
    vfh.second[i] = point.points[0].histogram[i];
  }
  vfh.first = path.string ();
  return (true);
}


/** \brief Search for the closest k neighbors
  * \param index the tree
  * \param model the query model
  * \param k the number of neighbors to search for
  * \param indices the resultant neighbor indices
  * \param distances the resultant neighbor distances
  */
inline void
nearestKSearch (flann::Index<flann::ChiSquareDistance<float> > &index, const vfh_model &model, 
                int k, flann::Matrix<int> &indices, flann::Matrix<float> &distances)
{
  // Query point
  flann::Matrix<float> p = flann::Matrix<float>(new float[model.second.size ()], 1, model.second.size ());
  memcpy (&p.ptr ()[0], &model.second[0], p.cols * p.rows * sizeof (float));

  indices = flann::Matrix<int>(new int[k], 1, k);
  distances = flann::Matrix<float>(new float[k], 1, k);
  index.knnSearch (p, indices, distances, k, flann::SearchParams (512));
  delete[] p.ptr ();
}

/** \brief Load the list of file model names from an ASCII file
  * \param models the resultant list of model name
  * \param filename the input file name
  */
bool
loadFileList (std::vector<vfh_model> &models, const std::string &filename)
{
  ifstream fs;
  fs.open (filename.c_str ());
  if (!fs.is_open () || fs.fail ())
    return (false);

  std::string line;
  while (!fs.eof ())
  {
    getline (fs, line);
    if (line.empty ())
      continue;
    vfh_model m;
    m.first = line;
    models.push_back (m);
  }
  fs.close ();
  return (true);
}

float fetchPar(std::string file) {

	std::ifstream infile;
	infile.open (file);
	std::string st;
    std::getline(infile,st);
	infile.close();
	float x;
	from_string<float>(x, st, std::dec);
	return x;
}

std::vector< poseApproach > fetchPoseApproaches(std::string file){

	std::vector< poseApproach > vec;
	poseApproach paTmp;
	int tmp;

	std::ifstream infile;
	infile.open (file);
	std::string st;
    std::getline(infile,st);  //First line is discarded

	while(!infile.eof()) {
		
		std::getline(infile,st);
		if(std::count(st.begin(), st.end(), ' ') != 1)
			continue;

		tmp=st.find_last_of(' ');
		paTmp.pose = st.substr(0, tmp);
		from_string<double>(paTmp.beta, st.substr(tmp+1, st.length()-1), std::dec);

		vec.push_back(paTmp);

	}


	infile.close();

	return vec;
}

std::vector< object > nearestNeighbors (int cluster_no)
{
  
  std::vector<object> objects;
  object tmp;

  std::vector<float> vecCentroid;
  std::vector<float> vecGrasp;
  std::vector<poseApproach> pa;
  std::vector<float> beta;
  std::vector<int> clusterNo;

  int flag;
  int k = 1;
  double thresh = 300;     
  float val[9];
  int no=0;
  float arr[24];
  std::string aa;
  std::string bb;
  std::string cc;


  for (int j=0; j<=cluster_no; j++) {  
	  convertVFH(j, &val[0]);

	  // Load the test histogram
	  vfh_model histogram;
	  if (!loadHist ("vfh_model.pcd", histogram))
	  {
		pcl::console::print_error ("Cannot load test file \n");
		return objects;
	  }

	  std::string kdtree_idx_file_name = "kdtree.idx";
	  std::string training_data_h5_file_name = "training_data.h5";
	  std::string training_data_list_file_name = "training_data.list";

	  std::vector<vfh_model> models;
	  flann::Matrix<int> k_indices;
	  flann::Matrix<float> k_distances;
	  flann::Matrix<float> data;
	  // Check if the data has already been saved to disk
	  if (!boost::filesystem::exists ("training_data.h5") || !boost::filesystem::exists ("training_data.list"))
	  {
		pcl::console::print_error ("Could not find training data models files %s and %s!\n", 
			training_data_h5_file_name.c_str (), training_data_list_file_name.c_str ());
		return objects;
	  }
	  else
	  {
		loadFileList (models, training_data_list_file_name);
		flann::load_from_file (data, training_data_h5_file_name, "training_data");
	  }

	  // Check if the tree index has already been saved to disk
	  if (!boost::filesystem::exists (kdtree_idx_file_name))
	  {
		pcl::console::print_error ("Could not find kd-tree index in file %s!", kdtree_idx_file_name.c_str ());
		return objects;
	  }
	  else
	  {
		flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("kdtree.idx"));
		index.buildIndex ();
		nearestKSearch (index, histogram, k, k_indices, k_distances);
	  }


		if (k_distances[0][0]<thresh) {
			pcl::console::print_error("\n\n\n    [CL. %d] ", no);


			aa = models.at (k_indices[0][0]).first.c_str() ;
			pcl::console::print_info ("%s distant %f\n",
				models.at (k_indices[0][0]).first.c_str (), k_distances[0][0]);
			std::cout<<std::showpos<<std::fixed<<"      X: "<<val[0]<<" to "<<val[1]<<"\n      Y: "<<
				val[2]<<" to "<<val[3]<<"\n      Z: "<<val[4]<<" to "<<val[5]<<"\n"<<std::setprecision(3);
			int tmp=aa.find_last_of('\\');

			bb=aa.substr(tmp+1,3);
			cc=aa.substr(tmp+1,aa.length());
			aa=aa.substr(0,tmp+1);

			bb.append(".txt");
			aa.append(bb);

			arr[4*no]=val[6];
			arr[4*no+1]=val[7];
			arr[4*no+2]=val[8];
			arr[4*no+3]=fetchPar(aa);

			pa = fetchPoseApproaches(aa);
			
			int ii;
			flag = 0;
			for(ii = 0; ii < pa.size(); ii++){

				if(!cc.compare(pa.at(ii).pose)){			
					beta.push_back(pa.at(ii).beta);
					flag = 1;
				}				

			}
			
			if(flag == 0)
				beta.push_back(-1);


			
			clusterNo.push_back(j);

			no++;

		}
  }

  vecGrasp = displayGrasp(&arr[0],no);



  int i;
  for( i = 0; i < no; i++){
	  
	  tmp.centroidX = arr[4*i];
	  tmp.centroidY = arr[4*i+1];
	  tmp.centroidZ = arr[4*i+2];

	  tmp.clusterNo = clusterNo.at(i);

	  objects.push_back(tmp);

  }

  

  return objects;

}